//
// Created by f on 2022/3/17.
//

#ifndef HPA_QUADTREE_MAP_H
#define HPA_QUADTREE_MAP_H

#include <memory>
#include <queue>
#include "map_base.h"

class QuadtreeNode{
public:

    /**
     * 用于root节点的构造函数，没有父节点
     * @param value 值
     * @param min_x 在grids中最左下网格的坐标
     * @param min_y
     * @param size node的边长是多少个网格
     */
    QuadtreeNode(int min_x, int min_y, int size);

    QuadtreeNode(std::weak_ptr<QuadtreeNode> father, int min_x, int min_y, int size);

    ~QuadtreeNode() = default;

    QuadtreeNode() = delete;
    QuadtreeNode(const QuadtreeNode & qn) = delete;
    QuadtreeNode & operator=(const QuadtreeNode & qn) = delete;

    /**
     * @brief 将节点设为最小叶子节点。最小叶子节点没有子节点
     * @param value
     * @return
     */
    void SetLeaf();

    bool isLeaf();

    const int& Min_x();
    const int& Min_y();
    const int& Size();

    std::shared_ptr<QuadtreeNode> lu_; //左上叶子
    std::shared_ptr<QuadtreeNode> ru_; //右上叶子
    std::shared_ptr<QuadtreeNode> ld_; //左下叶子
    std::shared_ptr<QuadtreeNode> rd_; //右下叶子
    std::weak_ptr<QuadtreeNode> father_;

private:
    //node在grids中的位置用它坐标最小的那个网格来表示
    int min_x_, min_y_;
    //node的边长是多少个网格。node只能是正方形
    int size_;
    uint8_t value_ = 0; //节点默认值为0
};

//QuadtreeMap网格坐标系，以左下网格为0，以上、右为x、y正方向
//注意这里row和x是一个意思，col和y是一个意思
class QuadtreeMap: public MapBase{
public:
    /**
     * 构造函数
     * @param center_x 地图中心在世界坐标系中的坐标
     * @param center_y
     * @param center_yaw
     * @param resolution 地图网格的尺寸
     * @param min_dim 要求的地图最小尺寸。实际生成的地图不小于这个尺寸
     * @param layer_id 地图ID。每个地图的网格只有0和1两种状态(0为默认)，所以也只能表示一层语义信息
     */
    QuadtreeMap(const double &center_x, const double &center_y, const double &center_yaw,
                const double &resolution, const double &min_dim,
                const int &layer_id);

    /**
     * 初始化一个空的地图对象，用于加载储存的地图文件
     * @param layer_id
     */
    QuadtreeMap(const int &layer_id);

    ~QuadtreeMap() = default;

    QuadtreeMap(const QuadtreeMap & qm) = delete;
    QuadtreeMap & operator=(const QuadtreeMap & qm) = delete;

    /**
     * 添加地图点。目前的设定是，只要对应的最小网格区域有一个点，网格的值就会被置为1
     * @param x 点在世界坐标系中的坐标
     * @param y
     * @return
     */
    bool AddPoint(const double& x, const double& y) override;

    /**
     * 找到世界坐标对应的值。只会是0或1
     * @param x
     * @param y
     * @return
     */
    uint8_t GetValue(const double& x, const double& y) override;

    bool SaveMap(const std::string& save_path,  const std::string& save_type) override;

    bool LoadMap(const std::string& load_path,  const std::string& load_type) override;

    void Clear() override;

    bool toCVMat(cv::Mat &m) override;

    double Center_x() override;
    double Center_y() override;
    double Center_yaw() override;
    double Dim_x() override;
    double Dim_y() override;
    int LayerID() override;

    ///层序打印tree，用于debug
    void ShowTree(){
        std::queue< std::shared_ptr<QuadtreeNode> > q;
        q.push(root_);
        while (!q.empty()){
            int curr_level_size = q.size();
            for(int i=0; i<curr_level_size; i++){
                auto node = q.front(); q.pop();

                if(node->isLeaf()) printf("1 ");
                else printf("N ");

                if(node->ru_ != nullptr) q.push(node->ru_);
                if(node->lu_ != nullptr) q.push(node->lu_);
                if(node->rd_ != nullptr) q.push(node->rd_);
                if(node->ld_ != nullptr) q.push(node->ld_);
            }
            printf("\n");
        }
    }

private:
    bool AddLeafNode(const int& min_x, const int& min_y, const int& size);

    std::shared_ptr<QuadtreeNode> root_;

    double center_x_, center_y_, center_yaw_;
    //地图只能是正方形，map_size_ = 2^max_depth_
    double grid_dim_; //最小方格边长，也就是地图的“像素”大小
    int map_size_ = 1; //整个地图边长为多少个网格
    int max_depth_ = 0; //最大深度。只有根节点则深度为0

    int layer_id_; //每个QuadtreeMap只有一层
};

#endif //HPA_QUADTREE_MAP_H
